#!/usr/bin/python2.5
# Test bot

import time
from nxt.sensor import *
from nxt.motor import *
from lib import robot_lib
from lib import shell

import conf

class Cricket(robot_lib.BasicRobot):
  """Test Robot."""

  def StartUp(self):
    self.Connect()
    self.left_motor = Motor(self.Brick(), PORT_C)
    self.right_motor = Motor(self.Brick(), PORT_A)
    self.motor_running = False

  def Run(self):
    print 'running - press button for actions; Ctrl-c to quit'
    try:
      while 1:
        if TouchSensor(self.Brick(), PORT_1).get_sample():
          if self.motor_running == False:
            print 'Start Your Engines!'

            self.left_motor.mode = MODE_MOTOR_ON
            self.left_motor.run_state = RUN_STATE_RUNNING
            self.left_motor.set_output_state()

            self.right_motor.mode = MODE_MOTOR_ON
            self.right_motor.run_state = RUN_STATE_RUNNING
            self.right_motor.set_output_state()
            self.motor_running = True
            time.sleep(1)

        if TouchSensor(self.Brick(), PORT_1).get_sample():
          if self.motor_running == True:
            print 'Stopping...'
            self.left_motor.mode = MODE_BRAKE
            self.left_motor.run_state = RUN_STATE_IDLE
            self.left_motor.set_output_state()

            self.right_motor.mode = MODE_BRAKE
            self.right_motor.run_state = RUN_STATE_IDLE
            self.right_motor.set_output_state()
            self.motor_running = False
            time.sleep(1)

    except KeyboardInterrupt:
      return

  def LeftMotor(self, running, reverse=False):
    if running:
      self.left_motor.mode = MODE_MOTOR_ON
      self.left_motor.run_state = RUN_STATE_RUNNING
    else:
      self.left_motor.mode = MODE_BRAKE
      self.left_motor.run_state = RUN_STATE_IDLE

    if reverse:
      self.left_motor.power = self._default_power * -1
    else:
      self.left_motor.power = self._default_power

    self.left_motor.set_output_state()

  def RightMotor(self, running, reverse=False):
    if running:
      self.right_motor.mode = MODE_MOTOR_ON
      self.right_motor.run_state = RUN_STATE_RUNNING
    else:
      self.right_motor.mode = MODE_BRAKE
      self.right_motor.run_state = RUN_STATE_IDLE

    if reverse:
      self.right_motor.power = self._default_power
    else:
      self.right_motor.power = self._default_power * -1

    self.right_motor.set_output_state()

  def TurnLeft(self, num_secs=1):
    self.LeftMotor(True)
    self.RightMotor(True)
    time.sleep(num_secs)
    self.LeftMotor(False)
    self.RightMotor(False)

  def TurnRight(self, num_secs=1):
    self.LeftMotor(True, reverse=True)
    self.RightMotor(True, reverse=True)
    time.sleep(num_secs)
    self.LeftMotor(False)
    self.RightMotor(False)

  def Forward(self, num_secs=1):
    self.LeftMotor(True, reverse=True)
    self.RightMotor(True)
    time.sleep(num_secs)
    self.LeftMotor(False)
    self.RightMotor(False)

  def Reverse(self, num_secs=1):
    self.LeftMotor(True)
    self.RightMotor(True, reverse=True)
    time.sleep(num_secs)
    self.LeftMotor(False)
    self.RightMotor(False)


def main():
  robot = Cricket('Jiminy', conf.MAC_ADDRESS)
  cmd_shell = shell.CommandShell(robot)
  #robot.StartUp()
  #robot.SetPower(65)

  cmd_shell.LoadCommandsFromModule('cricket_commands')
  cmd_shell.Interactive()
  #print 'Battery: %s mv' %  robot.brick.get_battery_level()
  #robot.Run()
  #robot.TurnLeft(2)
  #robot.TurnRight(2)
  #robot.Forward(1)
  #robot.Reverse()
  #robot.ShutDown()

  print '\nbye!'


if __name__ == '__main__':
  main()
